/**
 * @file RoboPID.cpp
 * @author Zifeng Huang (Mitcher2022@outlook.com)
 * @brief PID Controller, Cascade Control, Fuzzy Control and more
 * @version 0.1
 * @date 2022-10-28
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include   "RoboPID.h"
namespace RoboPid
{
    PID::PID(double _Kp, double _Ki, double _Kd, double Max, double Min, double _targetVal){
		Kp = _Kp;
		Kd = _Kd;
		Ki = _Ki;
		MaxOutput = Max;
		MinOutput = Min;		
		targetVal = _targetVal;
	}
	void PID::SetTarget(double _targetVal){
		targetVal = _targetVal;
	}
	double PID::Controller(double CurrentVal){
		ek0 = targetVal - CurrentVal;
		Output += Kp * (ek0 - ek1) + Ki * ek0 + Kd * (ek0 -2*ek1 + ek2);
		if(Output > MaxOutput){
			Output = MaxOutput;
		}
		if(Output < MinOutput){
			Output = MinOutput;
		}
		ek2 = ek1;
		ek1 = ek0;
		return Output;
	}
}